#include "Estimator.h"

#define QUEUE_SIZE 3

Estimator::Estimator(){//Initialise Estimation Components


        //TODO: Get from config?
        noTrendInterval = 60;
        delayStateInterval = 300;
        currentEstimate = 10; //Default value

        timeDifference[0] = timeDifference[1] = 0;

        time(&trendTimer[0]);
        time(&trendTimer[1]);
        time(&delayTimer[0]);
        time(&delayTimer[1]);


}

float Estimator::GetEstimate(float avg){
        time(&trendTimer[0]);
        time(&delayTimer[0]);
        timeDifference[0] += difftime(trendTimer[0], trendTimer[1]);//Timer for Delayed Message
        timeDifference[1] += difftime(delayTimer[0], delayTimer[1]);//Timer for no recent trend detected
        time(&trendTimer[1]);
        time(&delayTimer[1]);

        averageQueue.push_front(avg);
        if (averageQueue.size() < QUEUE_SIZE) return avg; //Don't compute estimate until we have enough data
        while(averageQueue.size() > QUEUE_SIZE) averageQueue.pop_back();

        switch (trend()){

            case TREND_UPWARD:
            cout << "UPWARD TREND\n";
            timeDifference[0] = timeDifference[1] = 0;
            return newEstimate();
            break;

            case TREND_DOWNWARD:
            cout << "DOWNWARD TREND\n";
            timeDifference[0] = 0;
            return newEstimate();
            break;

            case TREND_NONE://Compute estimate if one hasn't been done for a while
            if (timeDifference[0] >= noTrendInterval){
            timeDifference[0] = 0;
            return newEstimate();
            }
            break;
        }

        //TODO: fully implement flags
        if (timeDifference[1] >= delayStateInterval){
             cout << "Vehicle is delayed\n";
             timeDifference[1] = 0;
        }

        return currentEstimate;
}

Trend Estimator::trend(){ //Inspects average queue for a consistent change (trend) in its elements
    float a,b = 0;
    Trend trd = TREND_NONE;

    list<float>::iterator it = averageQueue.begin();
    //cout << "avgQueue size: " << averageQueue.size() << "\t" << *it << "\n";

    do{

        a = *it; it++; b = *it;
        cout << "\t\t\t" << b << "\n";
        if (b < a){
            if (trd == TREND_UPWARD || trd == TREND_NONE){//If averages are climbing,
                trd = TREND_UPWARD;                       //assume trend is forming...
                continue;
                }
            else{
                trd = TREND_NONE;//otherwise, bail out
                break;
            }
        }

        if (b > a){
            if (trd == TREND_DOWNWARD || trd == TREND_NONE){//and vice versa, if averages
                trd = TREND_DOWNWARD;                       //are falling
                continue;
            }
            else{
                trd = TREND_NONE;
                break;
            }
        }
    }while (*it != averageQueue.back());

    return trd;
}

float Estimator::newEstimate(){//Take an average of all the averages in the queue and use for the new estimate
    float est = 0;
    for (list<float>::iterator it = averageQueue.begin(); it != averageQueue.end(); it++) est += *it;
    currentEstimate = est / QUEUE_SIZE;
    return est / QUEUE_SIZE;
}
